Hierarchical Linear/Constant Time SLAM Using Particle Filters for Dense Maps

We present an improvement to the DP-SLAM algorithm for simultaneous localization and mapping (SLAM) that maintains multiple hypotheses about densely populated maps (one full map per particle in a particle filter) in time that is linear in all significant algorithm parameters and takes constant (amortized) time per iteration. This means that the asymptotic complexity of the algorithm is no greater than that of a pure localization algorithm using a single map and the same number of particles. We also present a hierarchical extension of DP-SLAM that uses a two level particle filter which models drift in the particle filtering process itself. The hierarchical approach enables recovery from the inevitable drift that results from using a finite number of particles in a particle filter and permits the use of DP-SLAM in more challenging domains, while maintaining linear time asymptotic complexity.